#ifndef BTL_DEMO_CAMERACALIBRATIONDEMO_QCAMERACALIBRATIONDEMOAPPLICATION
#define BTL_DEMO_CAMERACALIBRATIONDEMO_QCAMERACALIBRATIONDEMOAPPLICATION

#include <QApplication>
#include <QTimer>
#include <QKeyEvent>
#include <btl/Camera/CameraModelPlumbBob.hpp>
#include <btl/extra/VideoSource/VideoSourceUEye.hpp>
#include <btl/extra/VideoSource/VideoSourceV4L2.hpp>
//#include <btl/extra/VideoSource/VideoSourceKinect.hpp>
#include <btl/extra/Gui/QImageView.hpp>
#include <btl/Utility/OpenCVSupport.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>
#include <iterator>

class QCameraCalibrationDemoApplication : public QApplication
{

    Q_OBJECT

public:

    QCameraCalibrationDemoApplication ( int& argc, char** argv ) :
        QApplication ( argc, argv ),
        _cameraHasBeenCalibrated ( false ),
        _bChessBoardCornersFoundThisFrame ( false ),
        _numStoredFrames ( 0 )
    {
        //_pVideoSource = new btl::extra::VideoSourceUEye();
        _pVideoSource = new btl::extra::VideoSourceV4L2 ( std::string ( "/dev/video0" ).c_str() );
        //_pVideoSource = new btl::extra::VideoSourceKinect(); // for calibrating kinect rgb camera, please refer to
        //another demo KinectCalibrationDemo
        _pTimer = new QTimer;
        _pOriginalView = new QCameraCalibrationImageView ( this );
        _pOriginalView->show();
        _frameSize = _pVideoSource->frameSize();
        connect ( _pTimer, SIGNAL ( timeout() ), this, SLOT ( update() ) );
        _pTimer->start ( 30 );
    }

    ~QCameraCalibrationDemoApplication() {}

protected Q_SLOTS:

    void update()
    {
        btl::ImageRegionConstRGB frame = _pVideoSource->getNextFrame();

        if ( !_cameraHasBeenCalibrated )
        {
            cv::Size boardSize ( _NUM_CORNERS_X, _NUM_CORNERS_Y );
            cv::Mat cvFrame ( btl::cvUnsafeUnconstImage ( frame ) );
            _bChessBoardCornersFoundThisFrame =
                cv::findChessboardCorners ( cvFrame, boardSize, _vCurrentCorners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS );
            btl::ImageRGB output ( _frameSize ( 0 ), _frameSize ( 1 ) );

            std::cout << "we are here" <<std::endl;
            std::cout << " # of corners are : " << _vCurrentCorners.size() <<std::endl;

            if ( _bChessBoardCornersFoundThisFrame )
            {
                cv::Mat cvFrameGrey;
                cv::cvtColor ( cvFrame, cvFrameGrey, CV_BGR2GRAY );
                cv::cornerSubPix ( cvFrameGrey, _vCurrentCorners, cv::Size ( 11, 11 ), cv::Size ( -1, -1 ), cv::TermCriteria ( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1 ) );
                cv::drawChessboardCorners ( cvFrame, boardSize, _vCurrentCorners, _bChessBoardCornersFoundThisFrame );
                int i = 0;

                for ( int y = 0; y < _frameSize ( 1 ); ++y )
                {
                    for ( int x = 0; x < _frameSize ( 0 ); ++x )
                    {
                        btl::PixelRGB pixel;
                        pixel.red() = cvFrame.data[i++];
                        pixel.green() = cvFrame.data[i++];
                        pixel.blue() = cvFrame.data[i++];
                        output[y][x] = pixel;
                    }
                }
            }
        }
        else
        {
            btl::ImageRGB undistorted ( _frameSize ( 0 ), _frameSize ( 1 ) );
            cv::Mat cvUndistorted;
            cv::remap ( btl::cvUnsafeUnconstImage ( frame ), cvUndistorted, _mapX, _mapY, cv::INTER_LINEAR, cv::BORDER_CONSTANT );
            int i = 0;

            for ( int y = 0; y < _frameSize ( 1 ); ++y )
            {
                for ( int x = 0; x < _frameSize ( 0 ); ++x )
                {
                    btl::PixelRGB pixel;
                    pixel.red() = cvUndistorted.data[i++];
                    pixel.green() = cvUndistorted.data[i++];
                    pixel.blue() = cvUndistorted.data[i++];
                    undistorted[y][x] = pixel;
                }
            }

            _pUndistortedView->setImage ( undistorted );
        }

        _pOriginalView->setImage ( frame );
    }

protected:

    class QCameraCalibrationImageView : public btl::extra::QImageViewRGB
    {

    public:

        QCameraCalibrationImageView ( QCameraCalibrationDemoApplication* pApp, QWidget* pParent = 0, Qt::WindowFlags flags = 0 ) :
            btl::extra::QImageViewRGB ( pParent, flags ),
            _pApp ( pApp )
        {}

    protected:

        void keyPressEvent ( QKeyEvent* event )
        {
            if ( event->key() == Qt::Key_Space )
            {
                _pApp->storeCurrentFrame();
            }
            else if ( event->key() == Qt::Key_Return )
            {
                _pApp->calibrateCamera();
            }
        }

        QCameraCalibrationDemoApplication* _pApp;

    };

    friend class QCameraCalibrationImageView;

    void storeCurrentFrame()
    {
        if ( !_cameraHasBeenCalibrated )
        {
            if ( _bChessBoardCornersFoundThisFrame )
            {
                _imagePoints.push_back ( _vCurrentCorners );
                ++_numStoredFrames;
                std::cout << "Frame stored for calibration. (" << _numStoredFrames << ")" << std::endl;
            }
            else
            {
                std::cout << "Could not store frame, no chessboard corners were found." << std::endl;
            }
        }
    }

    void calibrateCamera()
    {
        if ( !_cameraHasBeenCalibrated )
        {
            std::vector<cv::Point3f> obj;

            for ( int j = 0; j < _NUM_CORNERS; ++j )
            {
                obj.push_back ( cv::Point3f ( j / _NUM_CORNERS_X, j % _NUM_CORNERS_X, 0.0f ) );
            }

            std::vector< std::vector<cv::Point3f> > objectPoints;

            for ( int i = 0; i < _numStoredFrames; ++i )
            {
                objectPoints.push_back ( obj );
            }

            _cameraMatrix = cv::Mat_<float> ( 3, 3 );
            _distCoeffs = cv::Mat_<float> ( 5, 1 );
            std::vector<cv::Mat> rvecs;
            std::vector<cv::Mat> tvecs;
            _cameraMatrix.ptr<float> ( 0 ) [0] = 1;
            _cameraMatrix.ptr<float> ( 1 ) [1] = 1;
            cv::Size cvFrameSize ( _frameSize ( 0 ), _frameSize ( 1 ) );
            cv::calibrateCamera ( objectPoints, _imagePoints, cvFrameSize, _cameraMatrix, _distCoeffs, rvecs, tvecs );
            std::cout << "Camera calibrated." << std::endl;
            std::cout << "Camera Matrix = (";

            for ( int y = 0; y < 3; ++y )
            {
                for ( int x = 0; x < 3; ++x )
                {
                    std::cout << _cameraMatrix.at<double> ( y, x );

                    if ( x != 2 )
                    {
                        std::cout << ",";
                    }
                }

                if ( y != 2 )
                {
                    std::cout << ";";
                }
            }

            std::cout << ")" << std::endl;
            std::cout << "Distortion Coefficients = (";

            for ( int k = 0; k < 5; ++k )
            {
                std::cout << _distCoeffs.at<double> ( k, 0 );

                if ( k != 4 )
                {
                    std::cout << ",";
                }
            }

            std::cout << ")" << std::endl;
            _cameraHasBeenCalibrated = true;
            _pCameraModel = new btl::CameraModelPlumbBob (
                _frameSize ( 0 ), _frameSize ( 1 ),
                _cameraMatrix.at<double> ( 0, 0 ), _cameraMatrix.at<double> ( 1, 1 ),
                _cameraMatrix.at<double> ( 0, 2 ), _cameraMatrix.at<double> ( 1, 2 ),
                _distCoeffs.at<double> ( 0, 0 ),
                _distCoeffs.at<double> ( 1, 0 ),
                _distCoeffs.at<double> ( 2, 0 ),
                _distCoeffs.at<double> ( 3, 0 ),
                _distCoeffs.at<double> ( 4, 0 ) );

            _mapX = cv::Mat_<float> ( cvFrameSize );
            _mapY = cv::Mat_<float> ( cvFrameSize );

            //create undistortion map:
            //for each pixel (x,y) in destination (undistorted) image,
            //compute its corresponding coordinates (u,v) in the original (distorted) image
            for ( int y = 0; y < _frameSize ( 1 ); ++y )
            {
                for ( int x = 0; x < _frameSize ( 0 ); ++x )
                {
                    Eigen::Vector2d undistorted ( x, y );
                    Eigen::Vector2d distorted = _pCameraModel->distortPoint ( undistorted );
                    _mapX[y][x] = ( float ) distorted ( 0 );
                    _mapY[y][x] = ( float ) distorted ( 1 );
                }
            }

            _pUndistortedView = new btl::extra::QImageViewRGB;
            _pOriginalView->setWindowTitle ( "Original Image" );
            _pUndistortedView->setWindowTitle ( "Undistorted Image" );
            _pUndistortedView->show();
        }
    }

    QTimer* _pTimer;

    btl::extra::VideoSource* _pVideoSource;

    Eigen::Vector2i _frameSize;

    btl::CameraModelDistorted* _pCameraModel;

    cv::Mat_<float> _mapX, _mapY;

    QCameraCalibrationImageView* _pOriginalView;

    btl::extra::QImageViewRGB* _pUndistortedView;

    static const int _NUM_CORNERS_X = 11;

    static const int _NUM_CORNERS_Y = 7;

    static const int _NUM_CORNERS = _NUM_CORNERS_X* _NUM_CORNERS_Y;

    bool _cameraHasBeenCalibrated;

    bool _bChessBoardCornersFoundThisFrame;

    int _numStoredFrames;

    std::vector<cv::Point2f> _vCurrentCorners;

    std::vector< std::vector<cv::Point2f> > _imagePoints;

    cv::Mat_<float> _cameraMatrix;

    cv::Mat_<float> _distCoeffs;

};

#endif //BTL_DEMO_CAMERACALIBRATIONDEMO_QCAMERACALIBRATIONDEMOAPPLICATION
